//
//  feather.cpp
//  template4
//
//  Created by Xiaochen Lian on 5/16/12.
//  Copyright (c) 2012 University of California, Los Angeles. All rights reserved.
//

#include "feather.h"
#include "Shapes.h"

CFeather::CFeather(const vec4 &pos, GLfloat width, GLfloat length)
: m_width(width), m_length(length)
{
    m_type = FEATHER;
    set_pos(pos);
    m_tfm = mat4(1.0);
}

void CFeather::build_model(const point4 &parent_mass_center)
{
    m_model_mat = Translate(m_model_pos);
    m_scale_mat = Scale(m_width, 0.5f, m_length);
    
    m_material.ambient = color4( 0.5f, 0.5f, 0.1f, 1.0f );
    m_material.diffuse = color4( 0.5f, 0.5f, 0.1f, 1.0f );
    m_material.specular = color4( 0.1f, 0.1f, 0.1f, 1.0f );
    m_material.shininess = 10.0f;  
    
    m_mass_center = m_model_mat * parent_mass_center;
}

void CFeather::set_material(const lighting_struct &material)
{
    m_material = material;
}

bool CFeather::is_right()
{
    return parent->is_right();
}

void CFeather::draw(const Angel::mat4 &tfm_parent)
{
    glUniform4fv(uAmbient, 1, g_env_lighting.ambient * m_material.ambient);
    glUniform4fv(uDiffuse, 1, g_env_lighting.diffuse.x * m_material.diffuse);
    glUniform4fv(uSpecular, 1, g_env_lighting.specular * m_material.specular);
    glUniform1f(uShininess, g_env_lighting.shininess * m_material.shininess);
    
    
    mat4 m = tfm_parent * m_model_mat * m_tfm;
//    m_mass_center = m*vec4(0.0, 0.0, 0.0, 1.0);
    drawCube(m * m_scale_mat);
}

vec4 CFeather::aerodynamics(const bird_status &cstatus, const mat4 &tfm, const double timestep)
{
    if (parent->get_type() == BODY)
    {
        mat4 T1 = Translate(0.0, 0.0, -m_length/2);
        mat4 T2 = Translate(0.0, 0.0, m_length/2);
        
        m_next_tfm = RotateZ(cstatus.tail_twist[0]);
        m_next_tfm = T2 * RotateX(cstatus.tail_bend[0]) * T1 * m_next_tfm;
    }
    else if (parent->get_type() == SHOULDER)
    {
        mat4 T1 = Translate(0.0, 0.0, -m_length/2);
        mat4 T2 = Translate(0.0, 0.0, m_length/2);
        
        if (is_right())
            m_next_tfm = T2 * RotateX(-cstatus.rshoulder_twist[0]) * T1;
        else
            m_next_tfm = T2 * RotateX(-cstatus.lshoulder_twist[0]) * T1;
    }

    mat4 m = tfm * m_model_mat * m_next_tfm;
    vec4 v, n, v_n, v_t, d, l, f_d, f_l;
    GLdouble theta;
    
    v = (m * vec4(0.0, 0.0, 0.0, 1.0) - m_mass_center) / timestep;
    v += cstatus.velocity;
    
    if (length(v) < 1e-4)
        return vec4(0.0);
    
    n = m * vec4(0.0, 1.0, 0.0, 0.0);
    
    d = -normalize(v);
    l = cross( normalize(cross(d, n)), d );
    l.w = 0.0f;
    
    v_n = dot(d,n) * n;
    v_t = d-v_n;
    theta = atan(dot(v_n, n) / length(v_t));
    
    f_l = aero_coef.rho * aero_coef.lift_coef(theta) * m_length * m_width * length(v) * l;
    f_d = aero_coef.rho * aero_coef.drag_coef(theta) * m_length * m_width * length(v) * d;
    
    return f_l+f_d;
//    return vec4(0.0);
}

void CFeather::update(const bird_status &cstatus, const mat4 tfm_parent)
{    
    m_tfm = m_next_tfm;
    m_mass_center = tfm_parent * m_model_mat * m_tfm * vec4(0.0, 0.0, 0.0, 1.0);
//    if (!is_right())
//        printf("%d %f %f %f\n", parent->get_type(), m_mass_center.x, m_mass_center.y, m_mass_center.z);
}
